/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.RobotMap;


/**
 *
 * @author Developer
 */
public class autonomous2 extends CommandBase {
    
    public autonomous2() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(camAim);
        requires(elevator);
        requires(shoot);
    }    
    // Called just before this Command runs the first time
//        aim obj_aim = new aim();
    protected void initialize() {
//        aimWithDrive obj_aimWithDrive=new aimWithDrive();
//        curTime = System.currentTimeMillis();
//        obj_aim.start();//this will initialize, execute repeatedly until isFinished return true.
//        obj_aimWithDrive.start();
        try {
            RobotMap.aimMotor.set(-1);
            Thread.sleep(2100);
            RobotMap.aimMotor.set(0);
    //        obj_aim.start();d
            Thread.sleep(2000);
            if(RobotMap.upperLimit.get())
            {
                                    System.out.println("Limit Switch");

                if (RobotMap.elevatorMotor.get() > 0)
                {
                    RobotMap.elevatorMotor.set(0);
                    RobotMap.loadMotor.set(0);
                    RobotMap.shooter1.set(0);
                }
            }
            else 
            {
                RobotMap.shooter1.set(1);
                RobotMap.loadMotor.set(1);
                RobotMap.elevatorMotor.set(.4);
            }
            //        long delay = 1500;//1500 msec
            //        if (System.currentTimeMillis()  == (curTime + delay))
        //        {
        //            RobotMap.aimMotor.set(-1);
        //        }
        }
        catch (InterruptedException ex)
        {
            ex.printStackTrace();
        }
        this.end();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
       
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
   
}
